#include "BsplineCurveEvaluator.h"
#include <cassert>

void BsplineCurveEvaluator::evaluateCurve(const std::vector<Point>& ptvCtrlPts, 
                                          std::vector<Point>& ptvEvaluatedCurvePts, 
                                          const float& fAniLength, 
                                          const bool& bWrap) const
{
    int iCtrlPtCount = ptvCtrlPts.size();

    //At least 2 Control points are needed (because I am adding "phantom" points for wrap).
    if(iCtrlPtCount >= 2) {

        int stepping = 75;
        ptvEvaluatedCurvePts.clear();

        //:Creating a new copy for the Control Points:
        std::vector<Point> ptvCtrlPtsB;
        //Creating "Phantom" Points at the beginning of the curve:
        if(bWrap) {
            //Mirror the last 2 Control Points on negative X:
            ptvCtrlPtsB.push_back(Point(-fAniLength+ptvCtrlPts[iCtrlPtCount-2].x, ptvCtrlPts[iCtrlPtCount-2].y));
            ptvCtrlPtsB.push_back(Point(-fAniLength+ptvCtrlPts[iCtrlPtCount-1].x, ptvCtrlPts[iCtrlPtCount-1].y));
        } else {
            //Add ControlPoints at the same Y as the 1st ControlPoint:
            ptvCtrlPtsB.push_back(Point(-4.0, ptvCtrlPts[0].y));
            ptvCtrlPtsB.push_back(Point(-4.0, ptvCtrlPts[0].y));
            ptvCtrlPtsB.push_back(Point(ptvCtrlPts[0].x, ptvCtrlPts[0].y));
            ptvCtrlPtsB.push_back(Point(ptvCtrlPts[0].x, ptvCtrlPts[0].y));
        }
        //Inserting the original ControlPoints:
        ptvCtrlPtsB.insert(ptvCtrlPtsB.end(), ptvCtrlPts.begin(), ptvCtrlPts.end() );

        //Creating "Phantom" Points at the end of the curve:
        if(bWrap) {
            //Mirror the 1st 2 ControlPoints on the right side:
            ptvCtrlPtsB.push_back(Point(ptvCtrlPts[0].x + fAniLength, ptvCtrlPts[0].y));
            ptvCtrlPtsB.push_back(Point(ptvCtrlPts[1].x + fAniLength, ptvCtrlPts[1].y));
        } else {
            //Add ControlPoints at the same Y as the last ControlPoints.
            ptvCtrlPtsB.push_back(Point(ptvCtrlPts[iCtrlPtCount-1].x, ptvCtrlPts[iCtrlPtCount-1].y));
            ptvCtrlPtsB.push_back(Point(ptvCtrlPts[iCtrlPtCount-1].x, ptvCtrlPts[iCtrlPtCount-1].y));
            ptvCtrlPtsB.push_back(Point(fAniLength+4, ptvCtrlPts[iCtrlPtCount-1].y));
            ptvCtrlPtsB.push_back(Point(fAniLength+4, ptvCtrlPts[iCtrlPtCount-1].y));
        }

        int iCtrlPtCountB = ptvCtrlPtsB.size();

        double a[4];
        double b[4];

        for(int i = 0; i < (iCtrlPtCountB-3); i++) {
            //Multiply ControlPoints with the BSpline Basis Matrix:
            a[0] = (-ptvCtrlPtsB[i].x + 3*ptvCtrlPtsB[i+1].x - 3*ptvCtrlPtsB[i+2].x + ptvCtrlPtsB[i+3].x)/6.0;
            a[1] = (3*ptvCtrlPtsB[i].x - 6*ptvCtrlPtsB[i+1].x + 3*ptvCtrlPtsB[i+2].x)/6.0;
            a[2] = (-3*ptvCtrlPtsB[i].x + 3*ptvCtrlPtsB[i+2].x)/6.0;
            a[3] = (ptvCtrlPtsB[i].x + 4*ptvCtrlPtsB[i+1].x + ptvCtrlPtsB[i+2].x)/6.0;

            b[0] = (-ptvCtrlPtsB[i].y + 3*ptvCtrlPtsB[i+1].y - 3*ptvCtrlPtsB[i+2].y + ptvCtrlPtsB[i+3].y)/6.0;
            b[1] = (3*ptvCtrlPtsB[i].y - 6*ptvCtrlPtsB[i+1].y + 3*ptvCtrlPtsB[i+2].y)/6.0;
            b[2] = (-3*ptvCtrlPtsB[i].y + 3*ptvCtrlPtsB[i+2].y)/6.0;
            b[3] = (ptvCtrlPtsB[i].y + 4*ptvCtrlPtsB[i+1].y + ptvCtrlPtsB[i+2].y)/6.0;

            //Interpolation:
            for(int i = 0; i < stepping ; i++) {
                float t;
                t = ((float)i)/((float)stepping);
                ptvEvaluatedCurvePts.push_back(Point( a[3] + t*(a[2] + t*(a[1] + t*a[0])),
                    b[3] + t*(b[2] + t*(b[1] + t*b[0])) ));
            }
        }       
    }
}